/***************************************************************************
 创建者: 华磊
 开始时间: 2019.6.13
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************/
#include "velocityprofile_improvedspline.h"
#include "velocityprofile_rect.hpp"
#include "velocityprofile_spline.hpp"
#include <QDebug>

#define IF_DEBUG_IMPROVED 0
#define D_NOMOVE_DISTANCE 0.001

namespace KDL {
VelocityProfile_ImprovedSpline::VelocityProfile_ImprovedSpline(double _maxvel, double _maxacc)
{
    if(IF_DEBUG_IMPROVED)
    {
        qDebug()<<"VelocityProfile_ImprovedSpline _maxvel"<<_maxvel<<"_maxacc"<<_maxacc;
    }

    maxvel=_maxvel;
    maxacc=_maxacc;
    velProfile_rect=new VelocityProfile_Rectangular(10000000);//不指定的话，会限制速度为０
    velProfile_start=new VelocityProfile_Spline();
    velProfile_end=new VelocityProfile_Spline();
    isOneVelProfile=false;
    isNoMoveFlag=false;
}

VelocityProfile_ImprovedSpline::~VelocityProfile_ImprovedSpline()
{
    delete velProfile_rect;
    delete velProfile_start;
    delete velProfile_end;
}
#define D_SPLINE_TIME_RATIO 2
int VelocityProfile_ImprovedSpline::SetProfile_transsition(double pos1, double vel1, double pos2, double &vel2,bool isStartChange)
{
    if(fabs(pos1-pos2)<D_NOMOVE_DISTANCE)
    {
        isNoMoveFlag=true;
        duration=0;
        startpos=pos1;
        endpos=pos2;
        return 1;
    }
    if(isStartChange)
    {
        motionStyle=E_VELOCITY_STYLE_CHANGE_CONSTANT;

    }
    else
    {
        motionStyle=E_VELOCITY_STYLE_CONSTANT_CHANGE;
    }


    startpos=pos1;
    endpos=pos2;
    startVel=vel1;
    endVel=vel2;


    double passPos1;
    double lineLength=fabs(pos1-pos2);

    //如果总长小于一定值，则直接用一段规划，因为可能距离太短，达不到匀速段。
    //估计需要加速距离
    if(vel2>vel1)
    {
        maxacc=fabs(maxacc);
    }
    else
    {
        maxacc=-fabs(maxacc);
    }
    double t_accUp=(vel2-vel1)/maxacc;
    double startLength;

    startLength=vel1*t_accUp+0.5*maxacc*t_accUp*t_accUp;

    //如果总长小于一定值，则直接用一段规划，因为可能距离太短，达不到vel2。
    if(IF_DEBUG_IMPROVED)
    {
       qDebug()<<"startLength"<<startLength<<"lineLength"<<lineLength<<"t_accUp"<<t_accUp<<"maxacc"<<maxacc;
    }


    if(startLength>=lineLength)
    {
        isOneVelProfile=true;
        double newTime;
        double vel_new;

        //达不到要求速度，则根据加速度计算实际速度。
        double x_t1=(-vel1+sqrt(vel1*vel1+4*0.5*maxacc*lineLength))/(2*0.5*maxacc);
        double x_t2=(-vel1-sqrt(vel1*vel1+4*0.5*maxacc*lineLength))/(2*0.5*maxacc);

        if(x_t1>0)
        {
           newTime=x_t1;
        }
        else if(x_t2>0)
        {
           newTime=x_t2;
        }
        else
        {
            newTime=0;
            if(IF_DEBUG_IMPROVED)
            {
               qDebug()<<"warnning::::VelocityProfile_ImprovedSpline newTime=0";
            }

        }
        vel_new=vel1+maxacc*newTime;
//        newTime=D_SPLINE_TIME_RATIO*newTime;
        if(false==isStartChange)//对于减速为０的情况，必须要减速为０
        {
            return -1;
        }
        //用2.6的系数可能把加速度拉的非常高
        //用3.8系数，VMARK定位偏差最大２ｃｍ  ???
        //
        velProfile_start->SetProfileDuration(pos1,vel1,pos2,vel_new,newTime);//3次样条曲线
//        velProfile_start->SetProfileDuration(pos1,vel1,0,pos2,vel_new,0,newTime);//4次样条曲线，有些情况会回退3秒
        duration=velProfile_start->Duration();
        vel2=vel_new;
        if(IF_DEBUG_IMPROVED)
        {
            qDebug()<<"isOneVelProfile pos1"<<pos1<<"vel1"<<vel1<<"pos2"<<pos2<<"vel_new"<<vel_new<<"newTime"<<newTime
                      <<"duration"<<duration;
        }


        return 1;
    }
    isOneVelProfile=false;
    if(isStartChange)
    {
        passPos1=startpos+startLength;
        velProfile_start->SetProfileDuration(pos1,vel1,0,passPos1,vel2,0,t_accUp);
        velProfile_rect->SetProfileDuration(passPos1,pos2,fabs((passPos1-pos2)/vel2));
        duration=velProfile_start->Duration()+velProfile_rect->Duration();
        if(IF_DEBUG_IMPROVED)
        {
            qDebug()<<"isStartChange pos1"<<pos1<<"vel1"<<vel1<<"passPos1"<<passPos1<<"vel2"<<vel2<<"t_accUp"<<t_accUp
                      <<"duration"<<duration;
        }


    }
    else
    {
        passPos1=startpos+pos2-pos1-startLength;
        velProfile_start->SetProfileDuration(passPos1,vel1,0,pos2,vel2,0,t_accUp);
        velProfile_rect->SetProfileDuration(pos1,passPos1,fabs((passPos1-pos1)/vel1));
        duration=velProfile_start->Duration()+velProfile_rect->Duration();
        if(IF_DEBUG_IMPROVED)
        {
            qDebug()<<"endchange pos1"<<pos1<<"vel1"<<vel1<<"passPos1"<<passPos1<<"vel2"<<vel2<<"t_accUp"<<t_accUp
                      <<"duration"<<duration;
        }

    }

    return 1;


}


void VelocityProfile_ImprovedSpline::SetProfile(double pos1, double pos2)
{
    if(IF_DEBUG_IMPROVED)
    {
        qDebug()<<"VelocityProfile_ImprovedSpline::SetProfile pos1"<<pos1<<"pos2"<<pos2<<"maxvel"<<maxvel<<"maxacc"<<maxacc;

    }
    if(fabs(pos1-pos2)<D_NOMOVE_DISTANCE)
    {
        isNoMoveFlag=true;
        duration=0;
        startpos=pos1;
        endpos=pos2;
        return;
    }
    motionStyle=E_VELOCITY_STYLE_ZERO_ZERO;
    startpos=pos1;
    endpos=pos2;

    double passPos1,passPos2;
    double lineLength=fabs(pos1-pos2);

     //如果总长小于一定值，则直接用一段规划，因为可能距离太短，达不到匀速段。

    //样条曲线段固定长度，后面可以改进，如果总轨迹很长，可以加长样条段。
    //估计需要加速距离
    double t_accUp=maxvel/maxacc;
    double startLength=0.5*maxacc*t_accUp*t_accUp;
    if(IF_DEBUG_IMPROVED)
    {
        qDebug()<<"startLength"<<startLength<<"lineLength"<<lineLength<<"maxacc"<<maxacc<<"t_accUp"<<t_accUp;
    }

    //如果总长小于一定值，则直接用一段规划，因为可能距离太短，达不到匀速段。
    if(startLength>=lineLength/2.0)
    {
        isOneVelProfile=true;
        //达不到要求速度，则根据加速度计算实际速度。
        double newTime=sqrt(2*lineLength/maxacc);//1.2米距离，加速度非常高
        newTime=D_SPLINE_TIME_RATIO*newTime;

        //用2.6的系数可能把加速度拉的非常高
        //用3.8系数，VMARK定位偏差最大２ｃｍ  ???
        //
        velProfile_start->SetProfileDuration(pos1,0,0,pos2,0,0,newTime);
        duration=velProfile_start->Duration();
        return ;
    }
    isOneVelProfile=false;
    passPos1=startpos+startLength;
    double secondLength=lineLength-startLength;
    passPos2=startpos+secondLength;
    velProfile_start->SetProfileDuration(pos1,0,0,passPos1,maxvel,0,t_accUp);
    velProfile_end->SetProfileDuration(passPos2,maxvel,0,pos2,0,0,t_accUp);
    double rectDuration=fabs(passPos1-passPos2)/maxvel;
    velProfile_rect->SetProfileDuration(passPos1,passPos2,rectDuration);
    duration=velProfile_start->Duration()+velProfile_end->Duration()+velProfile_rect->Duration();
    if(IF_DEBUG_IMPROVED)
    {
        qDebug()<<"duration"<<duration<<velProfile_start->Duration()<<velProfile_end->Duration()<<velProfile_rect->Duration();

    }
}

void VelocityProfile_ImprovedSpline::SetProfile_ZeroDuration(double pos1, double pos2, double newduration)
{
    if(IF_DEBUG_IMPROVED)
    {
        qDebug()<<"VelocityProfile_ImprovedSpline::SetProfile_ZeroDuration pos1"<<pos1<<"pos2"<<pos2;
    }
    if(fabs(pos1-pos2)<D_NOMOVE_DISTANCE)
    {
        isNoMoveFlag=true;
        duration=0;
        startpos=pos1;
        endpos=pos2;
        return;
    }

    motionStyle=E_VELOCITY_STYLE_ZERO_ZERO;
    startpos=pos1;
    endpos=pos2;
    //todo 需要根据公司计算匀速速度。

//    double passPos1,passPos2;
//    double lineLength=fabs(pos1-pos2);

//     //如果总长小于一定值，则直接用一段规划，因为可能距离太短，达不到匀速段。

//    //样条曲线段固定长度，后面可以改进，如果总轨迹很长，可以加长样条段。
//    //估计需要加速距离
//    double t_accUp=maxvel/maxacc;
//    double startLength=0.5*maxacc*t_accUp*t_accUp;
//    //如果总长小于一定值，则直接用一段规划，因为可能距离太短，达不到匀速段。
//    if(t_accUp>=newduration/2.0)
//    {
//        isOneVelProfile=true;
//        velProfile_start->SetProfileDuration(pos1,0,0,pos2,0,0,newduration);
//        duration=velProfile_start->Duration();
//        return ;
//    }
//    isOneVelProfile=false;
//    passPos1=startpos+startLength;
//    double secondLength=lineLength-startLength;
//    passPos2=startpos+secondLength;
//    velProfile_start->SetProfileDuration(pos1,0,0,passPos1,maxvel,0,t_accUp);
//    velProfile_end->SetProfileDuration(passPos2,maxvel,0,pos2,0,0,t_accUp);
//    double rectDuration=fabs(passPos1-passPos2)/maxvel;
//    velProfile_rect->SetProfileDuration(passPos1,passPos2,rectDuration);
//    duration=velProfile_start->Duration()+velProfile_end->Duration()+velProfile_rect->Duration();
//    qDebug()<<"duration"<<duration<<velProfile_start->Duration()<<velProfile_end->Duration()<<velProfile_rect->Duration();
}

void VelocityProfile_ImprovedSpline::SetProfile_tozero(double pos1, double vel1, double pos2)
{
    return ;
    motionStyle=E_VELOCITY_STYLE_CHANGE_CONSTANT_ZERO;
    startpos=pos1;
    endpos=pos2;

    double passPos1,passPos2;
    double lineLength=fabs(pos1-pos2);

     //如果总长小于一定值，则直接用一段规划，因为可能距离太短，达不到匀速段。
    //估计需要加速距离
//    if(vel2>vel1)
//    {
//        maxacc=fabs(maxacc);
//    }
//    else
//    {
//        maxacc=-fabs(maxacc);
//    }
    double t_accUp1=(maxvel-vel1)/maxacc;
    double startLength1=vel1*t_accUp1+0.5*maxacc*t_accUp1*t_accUp1;
    //样条曲线段固定长度，后面可以改进，如果总轨迹很长，可以加长样条段。
    //估计需要加速距离
    double t_accUp2=maxvel/maxacc;
    double startLength2=fabs(0.5*maxacc*t_accUp2*t_accUp2);
    double startLength=startLength1+startLength2;
    //如果总长小于一定值，则直接用一段规划，因为可能距离太短，达不到匀速段。
    if(startLength>=lineLength)
    {
        //todo 求解ｔ1 t2的公式有点复杂，也不好估计该值。
        isOneVelProfile=true;
        //达不到要求速度，则根据加速度计算实际速度。
        double newTime=sqrt(2*lineLength/maxacc);//1.2米距离，加速度非常高
        newTime=D_SPLINE_TIME_RATIO*newTime;

        //用2.6的系数可能把加速度拉的非常高
        //用3.8系数，VMARK定位偏差最大２ｃｍ  ???
        //
        velProfile_start->SetProfileDuration(pos1,0,0,pos2,0,0,newTime);
        duration=velProfile_start->Duration();
        return ;
    }
    isOneVelProfile=false;
    passPos1=startpos+startLength;
    double secondLength=lineLength-startLength;
    passPos2=startpos+secondLength;
    velProfile_start->SetProfileDuration(pos1,0,0,passPos1,maxvel,0,t_accUp1);
    velProfile_end->SetProfileDuration(passPos2,maxvel,0,pos2,0,0,t_accUp1);
    velProfile_rect->SetProfileDuration(passPos1,passPos2,fabs(passPos1-passPos2)/maxvel);
    duration=velProfile_start->Duration()+velProfile_end->Duration()+velProfile_rect->Duration();
}



void VelocityProfile_ImprovedSpline::SetProfileDuration(double pos1, double pos2, double newduration)
{
    if(IF_DEBUG_IMPROVED)
    {
        qDebug()<<"VelocityProfile_ImprovedSpline::SetProfileDuration duration"<<newduration;
    }

    if(fabs(pos1-pos2)<D_NOMOVE_DISTANCE)
    {
        isNoMoveFlag=true;
        duration=0;
        startpos=pos1;
        endpos=pos2;
        return;
    }

    motionStyle=E_VELOCITY_STYLE_ZERO_ZERO;
    startpos=pos1;
    endpos=pos2;

    isOneVelProfile=true;
    velProfile_start->SetProfileDuration(pos1,0,0,pos2,0,0,newduration);
    duration=velProfile_start->Duration();
    if(IF_DEBUG_IMPROVED)
    {
        qDebug()<<"duration"<<duration;
    }

    return ;

}

void VelocityProfile_ImprovedSpline::SetProfileVelocity(double pos1, double vel1, double pos2, double vel2, double newvelocity)
{

}

void VelocityProfile_ImprovedSpline::SetMax(double _maxvel, double _maxacc)
{
    maxvel=_maxvel;
    maxacc=_maxacc;

}

double VelocityProfile_ImprovedSpline::Duration() const
{
    if(isNoMoveFlag)
    {
        return 0;
    }
    return duration;
}

double VelocityProfile_ImprovedSpline::Pos(double time) const
{
    if(isNoMoveFlag)
    {
        qDebug()<<"warn:VelocityProfile_ImprovedSpline noMoveFlag true";
        return endpos;
    }
    switch(motionStyle)
    {
    case E_VELOCITY_STYLE_ZERO_ZERO:
    {
        if(isOneVelProfile)
        {
            return velProfile_start->Pos(time);
        }
        else
        {
            if(time<=velProfile_start->Duration())
            {
                return velProfile_start->Pos(time);
            }
            else if(time<=velProfile_start->Duration()+velProfile_rect->Duration())
            {
                return velProfile_rect->Pos(time-velProfile_start->Duration());
            }
            else
            {
                return velProfile_end->Pos(time-velProfile_start->Duration()-velProfile_rect->Duration());
            }
        }
        break;

    }
    case E_VELOCITY_STYLE_CHANGE_CONSTANT:
    {
        if(isOneVelProfile)
        {
            if(IF_DEBUG_IMPROVED)
            {
                qDebug()<<" isOneVelProfile velProfile_start->Pos(time)"<<velProfile_start->Pos(time)<<"time"<<time
                       <<"duration"<<velProfile_start->Duration();
            }

            double tmpPos=velProfile_start->Pos(time);
            if(tmpPos<0)
            {
                if(IF_DEBUG_IMPROVED)
                {
                    qDebug()<<" 1warnning,,, isOneVelProfile velProfile_start->Pos(time)"<<velProfile_start->Pos(time)<<"time"<<time;

                }
                tmpPos=0;
            }
            return tmpPos;
        }
        else
        {
            if(time<=velProfile_start->Duration())
            {
//                qDebug()<<"velProfile_start->Pos(time)"<<velProfile_start->Pos(time);
                return velProfile_start->Pos(time);
            }
            else
            {
                return velProfile_rect->Pos(time-velProfile_start->Duration());
            }
        }
        break;
    }
    case E_VELOCITY_STYLE_CONSTANT_CHANGE:
    {
        if(isOneVelProfile)
        {
            if(IF_DEBUG_IMPROVED)
            {
                qDebug()<<" isOneVelProfile velProfile_start->Pos(time)"<<velProfile_start->Pos(time)<<"time"<<time
                       <<"duration"<<velProfile_start->Duration();
            }

            double tmpPos=velProfile_start->Pos(time);
            if(tmpPos<0)
            {
                if(IF_DEBUG_IMPROVED)
                {
                    qDebug()<<" 2warnning,,, isOneVelProfile velProfile_start->Pos(time)"<<velProfile_start->Pos(time)<<"time"<<time;

                }
                tmpPos=0;
            }
            return tmpPos;
        }
        else
        {
            if(time<=velProfile_rect->Duration())
            {
                return velProfile_rect->Pos(time);
            }
            else
            {
                return velProfile_start->Pos(time-velProfile_rect->Duration());
            }
        }
        break;
    }
    }

    return startpos;

}

double VelocityProfile_ImprovedSpline::Vel(double time) const
{
    if(isNoMoveFlag)
    {
        return 0;
    }
    switch(motionStyle)
    {
    case E_VELOCITY_STYLE_ZERO_ZERO:
    {
        if(isOneVelProfile)
        {
            return velProfile_start->Vel(time);
        }
        else
        {
            if(time<=velProfile_start->Duration())
            {
                return velProfile_start->Vel(time);
            }
            else if(time<=velProfile_start->Duration()+velProfile_rect->Duration())
            {
                return velProfile_rect->Vel(time-velProfile_start->Duration());
            }
            else
            {
                return velProfile_end->Vel(time-velProfile_start->Duration()-velProfile_rect->Duration());
            }
        }
        break;

    }
    case E_VELOCITY_STYLE_CHANGE_CONSTANT:
    {
        if(isOneVelProfile)
        {
            return velProfile_start->Vel(time);
        }
        else
        {
            if(time<=velProfile_start->Duration())
            {
                return velProfile_start->Vel(time);
            }
            else
            {
                return velProfile_rect->Vel(time-velProfile_start->Duration());
            }
        }
        break;
    }
    case E_VELOCITY_STYLE_CONSTANT_CHANGE:
    {
        if(isOneVelProfile)
        {
            return velProfile_start->Vel(time);
        }
        else
        {
            if(time<=velProfile_rect->Duration())
            {
                return velProfile_rect->Vel(time);
            }
            else
            {
                return velProfile_start->Vel(time-velProfile_rect->Duration());
            }
        }
        break;
    }
    }

    return startVel;
}

double VelocityProfile_ImprovedSpline::Acc(double time) const
{
    if(isNoMoveFlag)
    {
        return 0;
    }
    switch(motionStyle)
    {
    case E_VELOCITY_STYLE_ZERO_ZERO:
    {
        if(isOneVelProfile)
        {
            return velProfile_start->Acc(time);
        }
        else
        {
            if(time<=velProfile_start->Duration())
            {
                return velProfile_start->Acc(time);
            }
            else if(time<=velProfile_start->Duration()+velProfile_rect->Duration())
            {
                return 0;
            }
            else
            {
                return velProfile_end->Acc(time-velProfile_start->Duration()-velProfile_rect->Duration());
            }
        }
        break;

    }
    case E_VELOCITY_STYLE_CHANGE_CONSTANT:
    {
        if(isOneVelProfile)
        {
            return velProfile_start->Acc(time);
        }
        else
        {
            if(time<=velProfile_start->Duration())
            {
                return velProfile_start->Acc(time);
            }
            else
            {
                return 0;
            }
        }
        break;
    }
    case E_VELOCITY_STYLE_CONSTANT_CHANGE:
    {
        if(isOneVelProfile)
        {
            return velProfile_start->Acc(time);
        }
        else
        {
            if(time<=velProfile_rect->Duration())
            {
                return 0;
            }
            else
            {
                return velProfile_start->Acc(time-velProfile_rect->Duration());
            }
        }
        break;
    }
    }

    return 0;
}

void VelocityProfile_ImprovedSpline::Write(std::ostream &os) const
{

}

VelocityProfile* VelocityProfile_ImprovedSpline::Clone() const
{
//    VelocityProfile_ImprovedSpline* tmpVel=new VelocityProfile_ImprovedSpline(maxvel,maxacc);
////    tmpVel->SetProfile(startpos,startVel,endpos,endVel);
//    return tmpVel;
//    return new VelocityProfile_ImprovedSpline();
}
}
